%POS

%% 清空环境
clc;
clear;
clc;

%% 参数初始化
m = 20; %种群个数
D = 3; %维度
N = 100; %d迭代次数

c1 = 2;
c2 = 2;
Jmax = (20 * pi / 180);
Jmin = -(20 * pi / 180);
Tmax = 4;
Tmin = 0.1;
Vmax = 2;
Vmin = -2;

Wmax = 0.9;
Wmin = 0.4;
%% 产生初始粒子和速度
for i = 1:m
    TimeSum(i, :) = 4 * rand(1, 3);
    V(i, :) = rands(1, 3);
    fitness(i) = TimeSum(i, 1) + TimeSum(i, 2) + TimeSum(i, 3);
end

%% 个体极值和群体极值
[gBest, gBestIndex] = min(fitness);
gBestGroup = TimeSum(gBestIndex, :); %全局最佳粒子群
pBestGroup = TimeSum; %个体最佳粒子
pBestFitnessGroup = fitness; %个体最佳值
gBestFitness = gBest; %全局最佳值

%% 迭代寻优
for i = 1:N
     w = (Wmax - i * (Wmax - Wmin) / N);
    for j = 1:m
        V(j, :) =w* V(j, :) + c1 * rand * (pBestGroup(j, :) - TimeSum(j, :)) + c2 * rand * (gBestGroup - TimeSum(j, :));

        %粒子群速度约束
        V(j, find(V(j, :) > Vmax)) = Vmax;
        V(j, find(V(j, :) < Vmin)) = Vmin;

        %粒子群位置约束
        TimeSum(j, :) = TimeSum(j, :) + V(j, :);
        TimeSum(j, find(TimeSum(j, :) > Tmax)) = Tmax;
        TimeSum(j, find(TimeSum(j, :) < Tmin)) = Tmin;

        %关节速度约束
        a(:, j) = Aa(TimeSum(j, 1), TimeSum(j, 2), TimeSum(j, 3));

        jv(1, j) = 3 * a(1, j) * TimeSum(j, 1)^2 + 2 * a(2, j) * TimeSum(j, 1) + a(3, j);
        jv(2, j) = 5 * a(5, j) * TimeSum(j, 2)^4 + 4 * a(6, j) * TimeSum(j, 2)^3 + 3 * a(7, j) * TimeSum(j, 2)^2 + 2 * a(8, j) * TimeSum(j, 2) + a(9, j);
        jv(3, j) = 3 * a(11, j) * TimeSum(j, 3)^2 + 2 * a(12, j) * TimeSum(j, 3) + a(13, j); %求出每个关节的速度

        if ((jv(1, j) > Vmax) || (jv(2, j) > Vmax) || (jv(3, j) > Vmax) || (jv(1, j) < Vmin) || (jv(2, j) < Vmin) || (jv(3, j) < Vmin))
            fitness(j) = 10;
        else
            fitness(j) = TimeSum(j, 1) + TimeSum(j, 2) + TimeSum(j, 3);
        end
        %个体最优更新
        if fitness(j) < pBestFitnessGroup(j)
            pBestGroup(j, :) = TimeSum(j, :);
            pBestFitnessGroup(j) = fitness(j);
        end

        %全局最优更新
        if fitness(j) < gBestFitness
            gBestGroup = TimeSum(j, :);
            gBestFitness = fitness(j);
        end
    end
    yy(i) = gBestFitness;
end

%% 多次多项式求解系数a
function H = Aa(t1, t2, t3)
x3 = 102.1321 * pi / 180;
x2 = 110.4723 * pi / 180;
x1 = 113.0098 * pi / 180;
x0 = 0;
format long
A = [t1^3, t1^2, t1, 1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0; ...
    3 * t1^2, 2 * t1, 1, 0, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0; ...
    6 * t1, 2, 0, 0, 0, 0, 0, -2, 0, 0, 0, 0, 0, 0; ...
    0, 0, 0, 0, t2^5, t2^4, t2^3, t2^2, t2, 1, 0, 0, 0, -1; ...
    0, 0, 0, 0, 5 * t2^4, 4 * t2^3, 3 * t2^2, 2 * t2, 1, 0, 0, 0, -1, 0; ...
    0, 0, 0, 0, 20 * t2^3, 12 * t2^2, 6 * t2, 2, 0, 0, 0, -2, 0, 0; ...
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, t3^3, t3^2, t3, 1; ...
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 3 * t3^2, 2 * t3, 1, 0; ...
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 6 * t3, 2, 0, 0; ...
    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; ...
    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; ...
    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; ...
    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1; ...
    0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0];
b = [0, 0, 0, 0, 0, 0, x3, 0, 0, x0, 0, 0, x2, x1]';
B = inv(A);
H = B * b;
end